/* Copyright (C) 2010 Fredrique Samuels, fredriquesamuels@gmail.com 

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "matrix.h"

namespace tri
{

std::ostream &operator<<( std::ostream& out, const TMatrix& m){
    out<<"<Matrix: ";
    for (int i = 0;i<16;i++){
        out<<" "<<m.elements[i];
    }
    out<<" >"<<std::endl;
    return out;
}

TMatrix::TMatrix()
{
    loadIdentity(this);
}

TMatrix::TMatrix(const float* e){
	for (int i=0; i<16; i++){
		elements[i] = e[i];
	}
}

TMatrix::~TMatrix()
{
}

void loadIdentity(tri::TMatrix* m){
    for (int i = 0;i<16;i++){
        m->elements[i] = 0;
        if( i == 0 ||
            i % 5 == 0){
            m->elements[i] = 1.0f;
        }
    }
}

void translate(TMatrix* m,float x,float y,float z){
    m->elements[12]+=x;
    m->elements[13]+=y;
    m->elements[14]+=z;
}

void getRotationMatrixX(float rot,TMatrix*m){
    loadIdentity(m);
    float d = (M_PI/180.0f) * rot;
    m->elements[5] = cos(d);
    m->elements[6] = -sin(d);
    m->elements[9] = sin(d);
    m->elements[10] = cos(d);
}

void getRotationMatrixY(float rot,TMatrix*m){    
    loadIdentity(m);
    float d = (M_PI/180.0f) * rot;
    m->elements[0] = cos(d);
    m->elements[2] = sin(d);
    m->elements[8] = -sin(d);
    m->elements[10] = cos(d);
}

void getRotationMatrixZ(float rot,TMatrix*m){
    loadIdentity(m);
    float d = (M_PI/180.0f) * rot;
    m->elements[0] = cos(d);
    m->elements[1] = -sin(d);
    m->elements[4] = sin(d);
    m->elements[5] = cos(d);
}

void getRotationMatrixXYZ(float x,float y,float z,TMatrix*m){
    
    TMatrix mx,my,mz,mxy;
    getRotationMatrixX(x,&mx);
    getRotationMatrixY(y,&my);
    getRotationMatrixZ(z,&mz);
    
    multMat(&mx,&my,&mxy);
    multMat(&mxy,&mz,m);
}

//static void get_rotation_TMatrixXYZ(float rot,TMatrix*m);

void multMat(const TMatrix*m1,const TMatrix*m2,TMatrix*res){
	TMatrix mat;
    loadIdentity(&mat);
    for(int i = 0;i<4;i++){
        for(int j = 0;j<4;j++){
        mat.elements[i*4+j] = m1->elements[i*4+0]*m2->elements[0+j] + 
                               m1->elements[i*4+1]*m2->elements[4+j] + 
                               m1->elements[i*4+2]*m2->elements[8+j] + 
                               m1->elements[i*4+3]*m2->elements[12+j];
        }
    }
    for(int i = 0;i<16;i++){
    	res->elements[i] = tri::utils::ABS(mat.elements[i]) < 0.001 ? 0 : mat.elements[i];
    }
    
}


void multVMat(const TMatrix* m,const TVector* v,TVector* dst){
    
    TMatrix rot,res;
    translate(&rot,v->x,v->y,v->z);
    multMat(&rot,m,&res);
    dst->x = res.elements[12];
    dst->y = res.elements[13];
    dst->z = res.elements[14];
}

void invert(float m[9]){
	float a= m[0];
	float b= m[1];
	float c= m[2];
	float d= m[3];
	float e= m[4];
	float f= m[5];
	float g= m[6];
	float h= m[7];
	float i= m[8];
	
	float det = 1.0 / ( a*(e*i-f*h) - b*(d*i-f*g) + c*(d*h-e*g) );
	m[0] = (e*i-f*h)/det;
	m[1] = (c*h-b*i)/det;
	m[2] = (b*f-c*e)/det;
	
	m[3] = (f*g-d*i)/det;
	m[4] = (a*i-c*g)/det;
	m[5] = (c*d-a*f)/det;

	m[6] = (d*h-e*g)/det;
	m[7] = (b*g-a*h)/det;
	m[8] = (a*e-b*d)/det;
}

void get3x3MatrixFromMatrix(const TMatrix* mat,float m[9]){
	m[0] = mat->elements[0];
	m[1] = mat->elements[1];
	m[2] = mat->elements[2];
	
	m[3] = mat->elements[4];
	m[4] = mat->elements[5];
	m[5] = mat->elements[6];
		
	m[6] = mat->elements[8];
	m[7] = mat->elements[9];
	m[8] = mat->elements[10];
}

void getMatrixFrom3x3Matrix(const float m[9],TMatrix* mat){
	loadIdentity(mat);
	mat->elements[0] = m[0];
	mat->elements[1] = m[1];
	mat->elements[2] = m[2];
	mat->elements[3] = 0;
	
	mat->elements[4] = m[3];
	mat->elements[5] = m[4];
	mat->elements[6] = m[5];
	mat->elements[7] = 0;
		
	mat->elements[8] = m[6];
	mat->elements[9] = m[7];
	mat->elements[10] = m[8];
	mat->elements[11] = 0;
	
	mat->elements[12] = 0;
	mat->elements[13] = 0;
	mat->elements[14] = 0;
	mat->elements[15] = 1;
	
}

}
